#include <UnitTest++.h>
#include <vector>
#include <Math/Vector2.hpp>
#include <Math/Vector3.hpp>
#include <Graphics/GeometryHelper.hpp>
#include <Graphics/Quaternion.hpp>

using namespace zzz;
using namespace std;

SUITE(GeometryHelperTest)
{
  TEST(PolygonToTriangles)
  {
    // 101       |-|
    // 1   ______| |______ 201
    //     |             |
    // 0   ------- -------
    //           | |
    // -100      |_|
    vector<Vector2d> p;
    p.push_back(Vector2d(0,0));
    p.push_back(Vector2d(100,0));
    p.push_back(Vector2d(100,-100));
    p.push_back(Vector2d(101,-100));
    p.push_back(Vector2d(101,0));
    p.push_back(Vector2d(201,0));
    p.push_back(Vector2d(201,1));
    p.push_back(Vector2d(101,1));
    p.push_back(Vector2d(101,101));
    p.push_back(Vector2d(100,101));
    p.push_back(Vector2d(100,1));
    p.push_back(Vector2d(0,1));
    vector<Vector3i> triangles;
    GeometryHelper::PolygonToTriangles(p,triangles);
//    for (zuint i=0; i<triangles.size(); i++)
//      zout<<triangles[i]<<endl;
  }

  TEST(PointInTriangle2D)
  {
    Vector2f t1(0,3), t2(4,0), t3(5,6);
    CHECK(!GeometryHelper::PointInTriangle2D(t1, t2, t3, Vector2f(1,1)));
    CHECK(GeometryHelper::PointInTriangle2D(t1, t2, t3, Vector2f(1,3)));
    CHECK(GeometryHelper::PointInTriangle2D(t1, t2, t3, Vector2f(2,3)));
    CHECK(GeometryHelper::PointInTriangle2D(t1, t2, t3, Vector2f(0.01,3)));
    CHECK(GeometryHelper::PointInTriangle2D(t1, t2, t3, Vector2f(4,1)));
    CHECK(!GeometryHelper::PointInTriangle2D(t1, t2, t3, Vector2f(4.01,0)));
  }

  TEST(PointInTriangle3D)
  {
    Vector3f t1(0,0,0), t2(4,0,4), t3(0,4,4);
    CHECK(GeometryHelper::PointInTriangle3D(t1, t2, t3, Vector3f(1,1,1)));
    CHECK(GeometryHelper::PointInTriangle3D(t1, t2, t3, Vector3f(1,1,1.1)));
    CHECK(GeometryHelper::PointInTriangle3D(t1, t2, t3, Vector3f(0,0,1.1)));
    CHECK(!GeometryHelper::PointInTriangle3D(t1, t2, t3, Vector3f(-1,0,1.1)));
    CHECK(GeometryHelper::PointInTriangle3D(t1, t2, t3, Vector3f(2,2,3.99)));
    CHECK(!GeometryHelper::PointInTriangle3D(t1, t2, t3, Vector3f(2,2,4.01)));
    CHECK(GeometryHelper::PointInTriangle3D(t1, t2, t3, Vector3f(2.01,2.01,3.89)));
    CHECK(!GeometryHelper::PointInTriangle3D(t1, t2, t3, Vector3f(2.01,2.01,4.01)));
  }

  TEST(Barycentric)
  {
    Vector2f t1(0,0), t2(0,4), t3(2,2);
    Vector3f b;
    GeometryHelper::Barycentric(t1,t1,t2,t3,b);
    CHECK_EQUAL(Vector3f(1,0,0),b);
    GeometryHelper::Barycentric(t2,t1,t2,t3,b);
    CHECK_EQUAL(Vector3f(0,1,0),b);
    GeometryHelper::Barycentric(t3,t1,t2,t3,b);
    CHECK_EQUAL(Vector3f(0,0,1),b);
    GeometryHelper::Barycentric((t1+t2)/2,t1,t2,t3,b);
    CHECK_EQUAL(Vector3f(0.5,0.5,0),b);
    GeometryHelper::Barycentric((t1+t2+t3)/3,t1,t2,t3,b);
    CHECK_CLOSE(0.333333333,b[0],EPSILON);
    CHECK_CLOSE(0.333333333,b[1],EPSILON);
    CHECK_CLOSE(0.333333333,b[2],EPSILON);
  }

  void TestRotate(Vector3f v1, Vector3f v2) {
    v1.Normalize();
    v2.Normalize();
    Vector3f axis = Cross(v1,v2);
    double angle = GeometryHelper::AngleFrom2Vectors(v1,v2);
    Quaternionf q(axis, angle);
    Vector3f v3=q.RotateVector(v1);
    CHECK_CLOSE(0, v3.DistTo(v2), EPSILON5);
  }

  TEST(AngleFrom2VectorsTest) 
  {
    TestRotate(Vector3f(0,0.1,1), Vector3f(0,0.1,-1));
    TestRotate(Vector3f(0.3,0.1,1), Vector3f(0.5,0.1,-1));
    TestRotate(Vector3f(20,0.5,1), Vector3f(-30,0.1,-1));
    TestRotate(Vector3f(7,6,8), Vector3f(-5, -3, -2));
  }

  TEST(TriangleArea2)
  {
    Vector2f v0(1,1), v1(1,3), v2(5,4);
    CHECK_EQUAL(4, GeometryHelper::TriangleArea(v0, v1, v2));
  }

  TEST(TriangleArea3)
  {
    Vector3f v0(3,0,0), v1(3,4,-3), v2(0,4,-3);
    CHECK_EQUAL(7.5, GeometryHelper::TriangleArea(v0, v1, v2));
  }

}